// --------------------------------------------------------------------------------------------------------------------
// <copyright file="PoseDiffTests.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Numerics
{
    using System;

    using Microsoft.Robotics.Numerics;
    using Microsoft.VisualStudio.TestTools.UnitTesting;

    using ShoNS.Array;

    /// <summary>
    /// Test methods of PoseDiff class
    /// </summary>
    [TestClass]
    public class PoseDiffTests
    {
        /// <summary>
        /// Test computing pose difference.
        /// </summary>
        [Priority(0)]
        [TestMethod]
        [TestCategory("Unit")]
        public void TestPoseDiff()
        {
            // test 1 - zero difference
            Matrix4 p1 = HomogenousTransform.CreateFromTranslation(new Vector3());

            PoseDiff poseDiff = new PoseDiff(p1, p1);
            Assert.AreEqual(poseDiff[0], 0);
            Assert.AreEqual(poseDiff[1], 0);
            Assert.AreEqual(poseDiff[2], 0);
            Assert.AreEqual(poseDiff[3], 0);
            Assert.AreEqual(poseDiff[4], 0);
            Assert.AreEqual(poseDiff[5], 0);

            // test 2 - normal rotation within x-y plane
            p1 = HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0, 0, 1), MathConstants.PIOverFour));
            p1[0, 3] = 1;
            p1[1, 3] = 0;
            p1[2, 3] = 2;
            Matrix4 p2 = HomogenousTransform.CreateFromAxisAngle(new AxisAngle(new Vector3(0, 0, 1), -MathConstants.PIOverFour));

            poseDiff = new PoseDiff(p1, p2);
            Assert.AreEqual(poseDiff[0], -Math.Sqrt(2.0) / 2, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseDiff[1], Math.Sqrt(2.0) / 2, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseDiff[2], -2);
            Assert.AreEqual(poseDiff[3], 0);
            Assert.AreEqual(poseDiff[4], 0);
            Assert.AreEqual(poseDiff[5], -MathConstants.PIOverTwo, MathConstants.ErrorEpsilon);

            // test 3 - a symmetric rotational matrix case
            double squreRootOfTwo = Math.Sqrt(2);
            p1 = HomogenousTransform.CreateFromTranslation(new Vector3());
            p2 = HomogenousTransform.CreateTransformation(new Matrix3(new double[,] { { squreRootOfTwo / 2.0, squreRootOfTwo / 2.0, 0 }, { squreRootOfTwo / 2.0, -squreRootOfTwo / 2.0, 0 }, { 0, 0, -1 } }), new Vector3(1, 2, 3));

            poseDiff = new PoseDiff(p1, p2);
            Vector3 axis = new Vector3(Math.Sqrt(squreRootOfTwo / 4.0 + 0.5), Math.Sqrt(0.5 - squreRootOfTwo / 4), 0);
            Assert.AreEqual(poseDiff[0], 1);
            Assert.AreEqual(poseDiff[1], 2);
            Assert.AreEqual(poseDiff[2], 3);
            Assert.AreEqual(poseDiff[3], axis.X * Math.PI, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseDiff[4], axis.Y * Math.PI, MathConstants.ErrorEpsilon);
            Assert.AreEqual(poseDiff[5], 0);

            // test 4 - another symmetric rotational matrix case
            p2 = HomogenousTransform.CreateTransformation(new Matrix3(new double[,] { { -1, 0, 0 }, { 0, -1, 0 }, { 0, 0, 1 } }), new Vector3(1, 2, 3));

            poseDiff = new PoseDiff(p1, p2);
            Assert.AreEqual(poseDiff[0], 1);
            Assert.AreEqual(poseDiff[1], 2);
            Assert.AreEqual(poseDiff[2], 3);
            Assert.AreEqual(poseDiff[3], 0);
            Assert.AreEqual(poseDiff[4], 0);
            Assert.AreEqual(poseDiff[5], Math.PI);
        }
    }
}
